// --------------------------------------------------------------------------------------------------------------------
// <copyright file="NavWaypointsFromSlamAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;

    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;

    /// <summary>
    /// Agent that creates waypoints file (used during navigation metrics) by correlating slam and gamepad messages and merging with the already created landmark-relative ground truth file.
    /// The workflow is like this:
    /// 1) Create an optimized SLAM map (this will output a groundTruthLandmarkRelative.xml file and a recorded stream of optimized slam messages.
    /// 2) Create a waypoints file with this agent which will merge the optimized slam messages at the landmarks (indicated by gamepad presses) with the ground truth into a NavWaypoints.xml file
    /// </summary>
    [DataContract]
    public class NavWaypointsFromSlamAgent : SynchronizedConsumerProducerAgent<GamePadAgentMessage, SLAM2DAgentMessage, CompletionMessage>
{
        /// <summary>
        /// The name of the output file to write waypoint data 
        /// </summary>
        public static readonly string WaypointsFileName = "NavWaypoints.xml";

        /// <summary>
        /// The maximum discrepancy between navigation and pose originating times.
        /// Images are captured every 1 seconds and the robot is instructed to wait 10 seconds at each waypoint.
        /// </summary>
        private const int DefaultSyncTolerance = 20;

        /// <summary>
        /// The name of the landmark relative ground truth file (generated during mapping) to merge with the slam data into the output file
        /// </summary>
        private const string GroundTruthLandmarkRelativeFileName = "groundTruthLandmarkRelative.xml";

        /// <summary>
        /// Path to the landmarks list file.  This is the list of landmarks that will be visited.
        /// </summary>
        private const string LandmarkListFileName = "LandmarkList.txt";

        /// <summary>
        /// Name of the agent producing game pad messages.
        /// </summary>
        [DataMember(Name = "GamepadProducer")]
        private readonly string gamePadProducerName = default(string);

        /// <summary>
        /// Name of the agent producing the optimized trajectory (Pose2D) messages.
        /// </summary>
        [DataMember(Name = "OptimizedTrajectoryProducer")]
        private readonly string optimizedTrajectoryProducer = default(string);

        /// <summary>
        /// Location of the recordings directory
        /// </summary>
        [DataMember(Name = "RecordingsDirectory")]
        private readonly string recordingsDirectory = default(string);

        /// <summary>
        /// The last GamePad state information.
        /// </summary>
        private GamePadAgentMessage.GamePadAgentButtons lastGamePadState;

        /// <summary>
        /// The landmarks ids we will visit
        /// </summary>
        private List<int> landmarkIds;

        /// <summary>
        /// The current landmark index
        /// </summary>
        private int landmarkIndex;

        /// <summary>
        /// The list of waypoints which get generated based on the slam input correlated with the gamepad
        /// </summary>
        private List<Waypoint> waypoints;

        /// <summary>
        /// The list of ground truth poses loaded from the file that is generated during the recording
        /// </summary>
        private List<Pose2D> groundTruthPoses;

        /// <summary>
        /// Initializes a new instance of the <see cref="NavWaypointsFromSlamAgent"/> class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="gamePadProducer">The gamepad message producer</param>
        /// <param name="slamProducer">Slam2D message producer</param>
        /// <param name="recordingsDirectory">The directory path to the recording files (ground truth files)</param>
        public NavWaypointsFromSlamAgent(
            string name,
            IProducer<GamePadAgentMessage> gamePadProducer,
            IProducer<SLAM2DAgentMessage> slamProducer,
            string recordingsDirectory)
            : base(name, DefaultSyncTolerance, false)
        {
            this.gamePadProducerName = gamePadProducer.Name;
            this.optimizedTrajectoryProducer = slamProducer.Name;
            this.recordingsDirectory = recordingsDirectory;
        }

        /// <summary>
        /// The name of the gamePad producer
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.gamePadProducerName; }
        }

        /// <summary>
        /// The name of the slam2D producer
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.optimizedTrajectoryProducer; }
        }

        /// <summary>
        /// Called when agent is initialized.
        /// </summary>
        /// <param name="locator">Agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
        }

        /// <summary>
        /// Acts as de-facto constructor, because the agent can be activated/deactivated multiple times in a metrics run
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.waypoints = new List<Waypoint>();

            // loop over all entries in the landmark list file and add them to our list of landmarks.
            this.landmarkIds = new List<int>();
            using (StreamReader fileReader = new StreamReader(Path.Combine(this.recordingsDirectory, LandmarkListFileName)))
            {
                while (!fileReader.EndOfStream)
                {
                    string line = fileReader.ReadLine();
                    if (string.IsNullOrEmpty(line) || line[0] == '#')
                    {
                        continue;
                    }

                    string[] data = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
                    this.landmarkIds.Add(int.Parse(data[0]));
                }
            }

            this.groundTruthPoses = DataLoader<List<Pose2D>>.ImportData(Path.Combine(this.recordingsDirectory, GroundTruthLandmarkRelativeFileName));

            if (this.groundTruthPoses.Count != this.landmarkIds.Count)
            {
                throw new InvalidDataException("The number of landmark IDs in the landmarkListFile does not equal the number of poses in the groundTruthLandmarkRelativeFile!");
            }
        }

        /// <summary>
        /// Acts as the de-facto destructor, because the agent can be activated/deactivated multiple times in a metrics run
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
        }

        /// <summary>
        /// Called when the agent receives a message
        /// </summary>
        /// <param name="gamepadMessage">The gamepad message</param>
        /// <param name="slamMessage">The slam2D message</param>
        protected override void Receive(GamePadAgentMessage gamepadMessage, SLAM2DAgentMessage slamMessage)
        {
            GlobalMap map = slamMessage.Map;

            if (this.landmarkIndex > this.landmarkIds.Count)
            {
                TraceOut.Warning(TraceContexts.Nav, "Gamepad button pressed, but there are no more more landmarks in the list");
                return;
            }

            if (this.lastGamePadState == null)
            {
                this.lastGamePadState = gamepadMessage.Buttons;
            }

            // left button is pressed when the robot stops at a marker position
            // when left button is pressed, there will be multiple messages coming all having left pressed
            // we are targeting at the last one, and the one after last one should be left not pressed
            if (this.lastGamePadState.LeftShoulder && !gamepadMessage.Buttons.LeftShoulder)
            {
                this.waypoints.Add(new Waypoint(slamMessage.Location, this.groundTruthPoses[this.landmarkIndex], true, this.landmarkIds[this.landmarkIndex], false)); 

                TraceOut.Info(
                    TraceContexts.Nav,
                    " Added Waypoint {0} for landmark {1}", 
                    this.landmarkIndex, 
                    this.landmarkIds[this.landmarkIndex]);

                this.landmarkIndex = (this.landmarkIndex + 1);

                this.SaveWaypointsToFile();
            }

            this.lastGamePadState = gamepadMessage.Buttons;
        }

        /// <summary>
        /// Save the current list of waypoints to a file.
        /// </summary>
        private void SaveWaypointsToFile()
        {
            DataLoader<Waypoint[]>.ExportData(WaypointsFileName, this.waypoints.ToArray());
        }
    }
}
